{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import numpy.linalg as la\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "\n",
    "import seaborn as sns\n",
    "sns.set(font_scale=2)\n",
    "sns.set_style(\"whitegrid\")\n",
    "np.seterr(divide='ignore');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# One dimensional nonlinear equations\n",
    "\n",
    "## Example 1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this activity, we will find the root of nonlinear equations using three different iterative methods. For each one, you should be thinking about cost and convergence rate.\n",
    "\n",
    "The iterative methods below can be applied to more complex equations, but here we will use a simple nonlinear equation of the form:\n",
    "\n",
    "$$f(x) = e^x - 2 $$\n",
    "\n",
    "The exact root that satisfies $f(x) = 0$ is $x = ln(2) \\approx 0.693147$. We can take a look at the function in the interval $[-2,2]$.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "a0 = -2\n",
    "b0 = 2\n",
    "\n",
    "x = np.linspace(a0, b0)\n",
    "\n",
    "def f(x):\n",
    "    return np.exp(x) - 2\n",
    "\n",
    "def df(x):\n",
    "    return np.exp(x)\n",
    "\n",
    "\n",
    "xtrue = np.log(2)\n",
    "\n",
    "plt.plot(x, f(x))\n",
    "plt.plot(xtrue,0,'ro')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### a) Bisection Method\n",
    "\n",
    "#### First we will run the iterative process for a few iterations:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "a = a0\n",
    "b = b0\n",
    "interval = np.abs(a - b)   \n",
    "errors = []\n",
    "\n",
    "fa = f(a)\n",
    "\n",
    "for i in range(12):\n",
    "    m = (a+b)/2\n",
    "    fm = f(m)   \n",
    "    if  np.sign(fa) == np.sign(fm):\n",
    "        a = m \n",
    "        fa = fm # this line is not really needed, \n",
    "        # since we only need the sign of a, and sign of a is the same as sign of m\n",
    "    else:\n",
    "        b = m\n",
    "    interval = np.abs(a - b)    \n",
    "    errors.append(interval)        \n",
    "    print(\"%10g \\t %10g \\t %12g\" % (a, b, interval))\n",
    "    \n",
    "print('exact root is = ', np.log(2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Now we will add a stopping criteria. \n",
    "\n",
    "Since we know the interval gets divided by two every iteration, how many iterations do we need to perform to achieve the tolerance $2^{-k}$?\n",
    "\n",
    "Note that only one function evaluation is needed per iteration!\n",
    "\n",
    "We can also verify the linear convergence, with C = 0.5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "a = a0\n",
    "b = b0\n",
    "interval = np.abs(a - b)   \n",
    "errors = [interval]\n",
    "\n",
    "fa = f(a)\n",
    "count = 0\n",
    "\n",
    "while count < 30 and interval > 2**(-4):\n",
    "    m = (a+b)/2\n",
    "    fm = f(m)   \n",
    "    if  fa*fm > 0:\n",
    "        a = m \n",
    "    else:\n",
    "        b = m\n",
    "    interval = np.abs(a - b)    \n",
    "    errors.append(interval)        \n",
    "    print(\"%10g \\t %10g \\t %12g %12g\" % (a, b, interval, interval/errors[-2]))\n",
    "    \n",
    "print('exact root is = ', np.log(2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(errors)\n",
    "plt.ylabel('Error (interval size)')\n",
    "plt.xlabel('Iterations')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What happens if you have multiple roots inside the interval? Bisection method will converge to one of the roots. Try to run the code snippet above using the function \n",
    "\n",
    "$$ f(x) = 0.5 x^2 - 2 $$\n",
    "\n",
    "Change the interval, and observe what happens."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### b) Newton's Method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x0 = 2\n",
    "r = 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = x0\n",
    "count = 0\n",
    "tol = 1e-6\n",
    "err = 1\n",
    "errors = [err]\n",
    "\n",
    "while count < 30 and err > tol:\n",
    "    x = x - f(x)/df(x)\n",
    "    err = abs(x-xtrue)\n",
    "    errors.append(err)\n",
    "    print('%10g \\t%10g \\t %.16e %.4g' % (x, f(x), err, errors[-1]/(errors[-2]**r) ))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### c) Secant Method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x0 = 2\n",
    "x1 = 8\n",
    "r = 1.618"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Need two initial guesses\n",
    "xbefore = x0\n",
    "x = x1\n",
    "count = 0\n",
    "tol = 1e-8\n",
    "err = 1\n",
    "errors = [err]\n",
    "\n",
    "while count < 30 and err > tol:\n",
    "\n",
    "    df_approx = (f(x)-f(xbefore))/(x-xbefore)\n",
    "    xbefore = x\n",
    "    x = x - f(x)/df_approx\n",
    "    err = abs(x-xtrue)\n",
    "    errors.append(err)\n",
    "    print('%10g \\t%10g \\t %.16e %.4g' % (x, f(x), err, errors[-1]/errors[-2]**r ))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### d) Using scipy functions for root finding:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import scipy.optimize as opt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "opt.root_scalar(f,bracket=[-2, 3],method='bisect')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "opt.root_scalar(f,bracket=[-2, 3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "opt.root_scalar(f,x0=3, fprime=df, method='newton')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "opt.root_scalar(f,x0=3,x1=4, method='secant')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Example 2)\n",
    "\n",
    "### a) Graphical convergence of Newton's Method"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's take a look at this other function\n",
    "\n",
    "$$f(x) = x^3 - x + 1 $$\n",
    "\n",
    "And we plot it in the interval $[-4,4]$.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def f(x):\n",
    "    return x**3 - x +1\n",
    "\n",
    "def df(x):\n",
    "    return 3*x**2 - 1\n",
    "\n",
    "xmesh = np.linspace(-4, 4, 100)\n",
    "plt.ylim([-5, 10])\n",
    "plt.plot(xmesh, f(xmesh))\n",
    "\n",
    "xexact = -1.324717957244746\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "guesses = [-.9]\n",
    "guesses = [1.5]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Evaluate this cell many times in-place (using Ctrl-Enter)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = guesses[-1] # grab last guess\n",
    "\n",
    "slope = df(x)\n",
    "\n",
    "# plot approximate function\n",
    "plt.plot(xmesh, f(xmesh))\n",
    "plt.plot(xmesh, f(x) + slope*(xmesh-x))\n",
    "plt.plot(x, f(x), \"o\")\n",
    "plt.xlim([-4, 4])\n",
    "plt.ylim([-5, 10])\n",
    "plt.axhline(0, color=\"black\")\n",
    "\n",
    "# Compute approximate root\n",
    "xnew = x - f(x) / slope\n",
    "guesses.append(xnew)\n",
    "print(xnew)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f(xnew)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(guesses)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "error = abs(np.array(guesses)-xexact)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "plt.plot(error)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### b) Graphical convergence of Secant Method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "guesses = [2, 1.7]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# grab last two guesses\n",
    "x = guesses[-1]\n",
    "xbefore = guesses[-2]\n",
    "\n",
    "slope = (f(x)-f(xbefore))/(x-xbefore)\n",
    "\n",
    "# plot approximate function\n",
    "plt.plot(xmesh, f(xmesh))\n",
    "plt.plot(xmesh, f(x) + slope*(xmesh-x))\n",
    "plt.plot(x, f(x), \"o\")\n",
    "plt.plot(xbefore, f(xbefore), \"o\")\n",
    "plt.ylim([-4, 4])\n",
    "plt.ylim([-3, 10])\n",
    "plt.axhline(0, color=\"black\")\n",
    "\n",
    "# Compute approximate root\n",
    "xnew = x - f(x) / slope\n",
    "guesses.append(xnew)\n",
    "print(xnew)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# N-Dimensional Nonlinear Equations"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will solve the following system of nonlinear equations:\n",
    "\n",
    "$$ x + 2y = 2 $$\n",
    "\n",
    "$$ x^2 + 4y^2 = 4 $$\n",
    "\n",
    "We will define our vector valued function ${\\bf f}$, which takes a vector as argument, with the components $x$ and $y$. We are trying to find the numerical (approximated) solution that safisfies:\n",
    "\n",
    "$${\\bf f} = \\begin{bmatrix} f_1 \\\\ f_2 \\end{bmatrix}\n",
    "          = \\begin{bmatrix} x + 2y - 2 \\\\ x^2 + 4y^2 - 4 \\end{bmatrix}\n",
    "          = \\begin{bmatrix} 0 \\\\ 0 \\end{bmatrix}$$\n",
    "          \n",
    "and the exact solution is $[0,1]$\n",
    "\n",
    "We will also define the gradient of ${\\bf f}$, $\\nabla{\\bf f}$, which we call the Jacobian."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Newton's method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def f(xvec):\n",
    "    x, y = xvec\n",
    "    return np.array([\n",
    "        x + 2*y -2,\n",
    "        x**2 + 4*y**2 - 4\n",
    "        ])\n",
    "\n",
    "def Jf(xvec):\n",
    "    x, y = xvec\n",
    "    return np.array([\n",
    "        [1, 2],\n",
    "        [2*x, 8*y]\n",
    "        ])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pick an initial guess."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = np.array([1, 2])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now implement Newton's method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for i in range(10):\n",
    "    s = la.solve(Jf(x), -f(x))\n",
    "    x = x + s\n",
    "x"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check if that's really a solution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f(x)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The cost is $O(n^3)$ per iteration, since the Jacobian changes every iteration. But how fast is the convergence?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "r = 2\n",
    "\n",
    "xtrue = np.array([0, 1])\n",
    "x = np.array([1, 2])\n",
    "errors = [la.norm(x)]\n",
    "\n",
    "while errors[-1] > 1e-12:\n",
    "    A = Jf(x)\n",
    "    s = la.solve(A, f(x))\n",
    "    x = x - s\n",
    "    err = la.norm(x-xtrue)\n",
    "    errors.append(err)\n",
    "    print(' %.16e \\t %.4g' % (err, errors[-1]/errors[-2]**r ))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Combining Newton's method with Bisection\n",
    "\n",
    "Newton's method features a quadratic convergence rate, but is not guaranteed to converge unless the algorithm is started sufficiently close to the root of a function $f$.  For example, the function\n",
    "\n",
    "$$ f(x) = \\tanh(20x)$$,\n",
    "\n",
    "where $\\tanh$ is the hyperbolic tangent function, has a single root at $x = 0$, but Newton's method will quickly diverge even at modest distances from the root."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create functions that evaluate this function and it's derivative.  Recall that:\n",
    "\n",
    "$$\\frac{d}{dz}\\tanh(z) = 1 - \\tanh^2(z)$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def f(x):\n",
    "    return np.tanh(20*x)\n",
    "\n",
    "def df(x):\n",
    "    return 20*(1 - f(x)**2)\n",
    "\n",
    "def df(x):\n",
    "    return 20*(1 - np.tanh(20*x)**2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the function and its derivative over the interval $[-2,2]$.  Use two different plots for the function and its derivative"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "a0 = -2\n",
    "b0 = 2\n",
    "\n",
    "xmesh = np.linspace(a0, b0,200)\n",
    "plt.plot(xmesh, f(xmesh))\n",
    "plt.xlabel('$x$')\n",
    "plt.ylabel('$y$')\n",
    "plt.title('$f(x)$')\n",
    "plt.show()\n",
    "plt.plot(xmesh,df(xmesh))\n",
    "plt.xlabel('$x$')\n",
    "plt.ylabel('$y$')\n",
    "plt.title('$f\\'(x)$')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should notice that $f'(x) \\approx 0$ for most values of the input $x$.  Newton's method, which is given by:\n",
    "\n",
    "$$ x_{k+1} = x_k - \\frac{f(x_k)}{f'(x_k)}$$\n",
    "\n",
    "will divide by a very small number unless $x_k \\approx 0$.\n",
    "\n",
    "Even with a starting guess of $x_0 = 0.06$, Newton's method will diverge."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "guesses = [0.06]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###### Evaluate the next cell many times in-place (using Ctrl-Enter)\n",
    "The green dot is the current guess, and the orange line is the corresponding tangent line.  The next iterate will be where the tangent line intersects $x$-axis."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = guesses[-1] # grab last guess\n",
    "slope = df(x)\n",
    "\n",
    "# plot approximate function\n",
    "plt.plot(xmesh, f(xmesh))\n",
    "plt.plot(xmesh, f(x) + slope*(xmesh-x))\n",
    "plt.plot(x, f(x), \"o\")\n",
    "plt.xlim([-2, 2])\n",
    "plt.ylim([-2, 2])\n",
    "plt.axhline(0, color=\"black\")\n",
    "\n",
    "# Compute approximate root\n",
    "xnew = x - f(x) / slope\n",
    "guesses.append(xnew)\n",
    "\n",
    "print('Next iterate will be: ', xnew)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "On the other hand, the bisection method will always find the root of this function, as long as the initial interval endpoints are of opposite sign.  It will not converge as fast as Newton's method, however.  To get the best of both worlds, we can combine the two as follows."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###### Algorithm\n",
    "We start with an initial interval $[a,b]$ such that $f(a)\\cdot f(b) < 0$ (so that the function values have opposite signs).  We set $c = \\text{argmin}_{z=a,b} |f(z)|$, i.e.\n",
    "\n",
    "$ c = a\\hspace{7mm} \\text{if  } |f(a)| < |f(b)|$\n",
    "\n",
    "$ c = b\\hspace{7mm} \\text{if  } |f(b)| < |f(a)|$\n",
    "\n",
    "and try to begin a Newton iteration from there (**Question**: why do we select $c$ in this way, why not the reversed inequalities?).  \n",
    "\n",
    "If the Newton iteration takes us outside the open interval $(a,b)$, then we are not getting any closer to the desired root, and may even be diverging.  In this case, we fall back to the bisection method. \n",
    "\n",
    "I.e. we first try:\n",
    "\n",
    "$m = c - \\frac{f(c)}{f'(c)}$\n",
    "\n",
    "and if $m \\geq b$ or $m \\leq a$, we scrap this value of $m$ and use the bisection method instead:\n",
    "\n",
    "$ m = \\frac{a + b}{2}$.\n",
    "\n",
    "\n",
    "Using the same criteria as the standard bisection method, we then update either $a$ or $b$ to have the value $m$.  We then repeat the process and choose the next value of $c = \\text{argmin}_{z=a,b} |f(z)|$.  We can terminate whenever $|f(c)| < \\text{tol}$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Write a function that combines the two ideas.  Your function should return the root, as well as a list of tuples that stores $|f(c)|$ and what kind of step was taken (`Newton` or `Bisection`):\n",
    "```\n",
    "steps = [(|f(c)| ,'Bisection' ),....,( |f(c)|, 'Newton')]\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# clear with hints\n",
    "def bisection_Newton(a0,b0,tol,f,df):\n",
    "    \n",
    "    \n",
    "            \n",
    "    return root, steps"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test this method on $f(x) = \\tanh(20x)$ for the initial interval $[a,b] = [-2,5]$.  Use a tolerance of $10^{-8}$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "a0 = -2\n",
    "b0 = 5\n",
    "tol = 1e-8\n",
    "root, steps = bisection_Newton(a0,b0,tol,f,df)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "steps"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "root"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# GPS Application: N-D Newton's Method\n",
    "Global Positioning System (GPS) uses 4 satellites to calculate the location of a GPS receiver on earth.  We construct a $xyz$-coordinate system, with the origin located at the center of the Earth.  Relative to this coordinate system, each satellite $i$ has position $(A_i,B_i,C_i)$.  In addition to this, each satellite keeps track of time relative to some reference value, which is denoted $t_i$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we define the values for 4 satellites using the rows of a `numpy`array.  $A_i$, $B_i$ and $C_i$ are measured in kilometers, and $t_i$ is measured in seconds:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Satellites = np.array([[15600, 7540, 20140, 0.07074],\n",
    "                      [18760, 2750, 18610, 0.07220],\n",
    "                      [17610, 14630, 13480, 0.07690],\n",
    "                      [19170, 610, 18390, 0.07242]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(Satellites[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can plot the satellites in 3D, along with a sphere of radius 6370 km (approximately the radius of the Earth)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import 3d plotting\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "\n",
    "# data for sphere\n",
    "u = np.linspace(0, 2 * np.pi, 100)\n",
    "v = np.linspace(0, np.pi, 100)\n",
    "\n",
    "# multiply by 6370 km\n",
    "x = 6370*np.outer(np.cos(u), np.sin(v))\n",
    "y = 6370*np.outer(np.sin(u), np.sin(v))\n",
    "z = 6370*np.outer(np.ones(np.size(u)), np.cos(v))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(12,12))\n",
    "ax = fig.add_subplot(111,projection='3d')\n",
    "\n",
    "ax.scatter(Satellites[:,0],Satellites[:,1], Satellites[:,2],s = 200)\n",
    "ax.plot_surface(x, y, z, color='b',alpha = 0.3,rstride = 1,cstride = 1,linewidth = 0.0,zorder = 10)\n",
    "ax.set_xlabel('x')\n",
    "ax.set_ylabel('y')\n",
    "ax.set_zlabel('z')\n",
    "ax.axes.xaxis.set_ticklabels([])\n",
    "ax.axes.yaxis.set_ticklabels([])\n",
    "ax.axes.zaxis.set_ticklabels([])\n",
    "ax.view_init(0,90)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The receiver on Earth has $xyz$ coordinates of it's own, as well as a time delay $d$ that measures the difference between its clock and the reference time.  In order to solve for the location of the receiver and measure it's delay, four nonlinear equations must be solved:\n",
    "\n",
    "$F_i(\\mathbf{X}) = F_i(x,y,z,d) = \\sqrt{(x - A_i)^2 + (y - B_i)^2 + (z - C_i)^2} - c(t_i - d),\\hspace{7mm} \\text{for  } i=1,2,3,4$\n",
    "\n",
    "Here, $c$ is the speed of light, 299792.456 km/s.  Then we want to solve the nonlinear system of equations\n",
    "\n",
    "$\\mathbf{F}(\\mathbf{X}) = \\mathbf{0}$,\n",
    "\n",
    "where $\\mathbf{F}(\\mathbf{X}) = (F_1(\\mathbf{X}),F_2(\\mathbf{X}),F_3(\\mathbf{X}),F_4(\\mathbf{X}))^T$ and $\\mathbf{X} = (x,y,z,d)^T$.\n",
    "\n",
    "### Newton's Method\n",
    "To solve the equations, we can start with an initial guess $\\mathbf{X}_0$ and compute a sequence of approximate solutions by Newton's method:\n",
    "\n",
    "$\\mathbf{J}(\\mathbf{X})\\mathbf{S}_k = -\\mathbf{F}(\\mathbf{X})$\n",
    "\n",
    "$\\mathbf{X}_{k+1} = \\mathbf{X}_k + \\mathbf{S}_k$\n",
    "\n",
    "$\\mathbf{J}(\\mathbf{X})$ is the Jacobian of $\\mathbf{F}$ evaluated at $\\mathbf{X}$ with entries given by $J_{ij} = \\frac{\\partial F_i}{\\partial X_j}$.\n",
    "\n",
    "For example, the partial derivative $\\frac{\\partial F_1}{\\partial X_1} $ is given by:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$\\frac{\\partial F_1}{\\partial X_1} = \\frac{\\partial F_1}{\\partial x} = \\frac{x - A_1}{\\sqrt{(x-A_1)^2 + (y - B_1)^2 + (z - C_1)^2}}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Write functions that compute both $\\mathbf{F}$ and $\\mathbf{J}$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "c = 299792.458\n",
    "\n",
    "def f(X):\n",
    "  \n",
    "    pass\n",
    "\n",
    "def J(X):\n",
    "\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's use an initial guess of a receiver at the South Pole with no time delay: $\\mathbf{X}_0 = (0,0,-6370,0)^T$.  Starting from this guess, use Newton's method to locate the receiver on Earth.  Store each updated guess in a list.  You can terminate when $\\|\\mathbf{F}(\\mathbf{X})\\|_2 < 10^{-3}$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "X = np.array([6370,0,0,0])\n",
    "X_hist = [X]\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "X_hist"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can run this next cell to plot how the guess changed for the first 3 steps:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "X0 = X_hist[0]\n",
    "X1 = X_hist[1]\n",
    "X2 = X_hist[2]\n",
    "X3 = X_hist[3]\n",
    "\n",
    "x = 6370*np.outer(np.cos(u), np.sin(v))\n",
    "y = 6370*np.outer(np.sin(u), np.sin(v))\n",
    "z = 6370*np.outer(np.ones(np.size(u)), np.cos(v))\n",
    "\n",
    "fig = plt.figure(figsize=(15,15))\n",
    "ax = fig.add_subplot(111,projection='3d')\n",
    "# Plot the surface\n",
    "ax.scatter(Satellites[:,0],Satellites[:,1], Satellites[:,2],s = 200)\n",
    "ax.plot_surface(x, y, z, color='b',alpha = 0.3,rstride = 1,cstride = 1,linewidth = 0.0,zorder = 10)\n",
    "ax.set_xlabel('x')\n",
    "ax.set_ylabel('y')\n",
    "ax.set_zlabel('z')\n",
    "ax.view_init(0,90)\n",
    "ax.scatter(X0[0],X0[1], X0[2],s=40,color='r',alpha = 1,zorder = 0)\n",
    "ax.scatter(X1[0],X1[1], X1[2],s=40,color='r',alpha = 1,zorder = 0)\n",
    "ax.scatter(X2[0],X2[1], X2[2],s=40,color='r',alpha = 1,zorder = 0)\n",
    "ax.scatter(X3[0],X3[1], X3[2],s=40,color='r',alpha = 1,zorder = 0)\n",
    "ax.axes.xaxis.set_ticklabels([])\n",
    "ax.axes.yaxis.set_ticklabels([])\n",
    "ax.axes.zaxis.set_ticklabels([])\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
